
/*
 * JRoverMotors.h
 *
 *  Created on: Nov 16, 2012
 *      Author: bouchier
 */
/*! \file jRoverMotors.h
 *  \brief Header file for the class which controlls jRover's motors
 */
#ifndef JROVERMOTORS_H_
#define JROVERMOTORS_H_

	void initMotors();
	void setAllDriveSpeed(float rqSpeed);
	void setAllSteerAngle(int rqDegrees);
	void updateAllMotors();
	void driveRover(float linear, float angular, bool quiet=true);


/*! \class JRoverMotors
 * \brief The class which manages jRover's motors
 *
 * This class manages jRover's motors as a group, and provides for driving and getting
 * telemetry from drive and steering motors.
 *
 * First, initMotors() should be called. This initializes all jRovers motors, including setting the
 * steering motors up as servos and zeroing them. initMotors() recognizes the configuration with only the
 * first two motors (left-middle steering & drive) present as Paul's test config.
 *
 * Next, the driveRover() method can be used to set the desired robot speed & angular velocity,
 * after which updateMotors() should be called every 50ms to run the PID loop. driveRover() can be
 * called thereafter at any time to change robot motion request.
 *
 * The class provides storage for drive & steering motor telemetry that is published to ROS. The
 * get(Drive|Steer)MotorsTelemetry() methods pull motor data out of each motor & put it into the
 * corrrect spot in the telemetry array. There is also a method for setting test data.
 */
class JRoverMotors {
public:
	JRoverMotors();
	~JRoverMotors() {}
	void initMotors();
	void updateAllMotors();
	void driveRover(float linear, float angular, bool quiet=true);
	void getDriveMotorTelemetry(int motNum, ControlledMotor *m);
	void getDriveMotorsTelemetry();
	void setTestDriveMotorTelemetry(float f);
	void getSteerMotorTelemetry(int motNum, ControlledMotor *m);
	void getSteerMotorsTelemetry();
	void setTestSteerMotorTelemetry(float f);

	/*! \var driveMotorTelemetry
	 * Order of motors is:
	 * 0: left front
	 * 1: left middle
	 * 2: left back
	 * 3: right front
	 * 4: right middle
	 * 5: right back
	 */
	float driveMotorTelemetry[6][4];	// [motorNum][rqSpeed, speed, distance, power]
	/*! \var steerMotorTelemetry
	 * Order of motors is:
	 * 0: left front
	 * 1: left middle
	 * 2: left back
	 * 3: right front
	 * 4: right middle
	 * 5: right back
	 */
	int steerMotorTelemetry[6][3];	// [motorNum][rqAngle, angle, power]
private:
	void driveAllSteerToStop();
	void driveAllSteerToCenter();
	void zeroAllEncoders();
};

#endif /* JROVERMOTORS_H_ */
